#pragma once
#include "constants.h"
#include "dubins.hpp"
#include "node3d.hpp"
#include "tools.hpp"
#include <algorithm>
#include <boost/heap/binomial_heap.hpp>
#include <chrono>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <iostream>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/path.hpp>
#include <ompl/base/State.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <tf2/utils.h>
#include <vector>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

typedef ompl::base::SE2StateSpace::StateType State;

class HybridAStar {
private:
    nav_msgs::msg::OccupancyGrid::Ptr image_map;
    int                               map_width, map_height;
    std::vector< bool >               map_data;
    Eigen::Vector3d                   start;
    Eigen::Vector3d                   goal;
    int                               motion_type;
    double                            lambda = 1.05;  // 3.0

    rclcpp::Node::SharedPtr                                       ros_node = rclcpp::Node::make_shared( "hastar_node" );
    rclcpp::Publisher< geometry_msgs::msg::PoseArray >::SharedPtr expand_node_publisher_;
    std::vector< geometry_msgs::msg::Pose >                       vec_expand_points;
    rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr           dubin_path_publisher_;
    rclcpp::Publisher< visualization_msgs::msg::Marker >::SharedPtr dubin_veh_shape_visual_publisher_;

    std::vector< Node3D > node_path;

    struct CompareNodes {
        bool operator()( const Node3D *lhs, const Node3D *rhs ) const {
            return lhs->getC() > rhs->getC();
        }
    };

    typedef boost::heap::binomial_heap< Node3D *, boost::heap::compare< CompareNodes > > priorityQueue;

public:
    HybridAStar();

    ~HybridAStar();

    void InitParam( const Eigen::Vector3d &start_, const Eigen::Vector3d &goal_, int motion_type_,
                    const nav_msgs::msg::OccupancyGrid::Ptr image_map_, std::vector< std::vector< bool > > &map_data_ );

    void showMapData();

    bool plan();

    std::vector< Eigen::Vector4d > returnPath();

private:
    Node3D *hybridAStarSearch( Node3D &start, Node3D &goal );

    Node3D *hybridAStarSearch( Node3D &start, const Node3D &goal, Node3D *nodes3D );

    void showExpandNode( const Node3D &node, int iterations );

    void showDubinPath( Node3D &start, const Node3D &goal );

    void showVehShape( const Node3D &node );

    void updateH( Node3D &start, const Node3D &goal );

    bool checkCollision( const Node3D *node );

    bool LineCheck( const Eigen::Vector2d &p1, const Eigen::Vector2d &p2 );

    Node3D *dubinsShot( Node3D &start, const Node3D &goal );

    void tracePath( const Node3D *node, int i = 0, std::vector< Node3D > path = std::vector< Node3D >() );

    void calcInterPoint( const Eigen::Vector3d &start_node, int dir_type, std::vector< Eigen::Vector4d > &path );
};